#include "sdk_project_config.h"
#include "app_motors.h"
#include "app_etmr.h"

/*----------通道定义----------*/
/*TIM2CH1，横向*/
#define CAPTURE_INST_1 (2)
#define CAPTURE_CHAN_1 (1)
/*TIM2CH0，纵向*/
#define CAPTURE_INST_2 (2)
#define CAPTURE_CHAN_2 (0)

uint16_t Period_1,Period_2;					//周期
uint16_t High_Duty_1,High_Duty_2;		//高边时间
/*----------LPF相关定义----------*/
typedef struct {
	int32_t Duty_In;
	int32_t Duty_Out;
	int32_t Duty_Alpha;
	int32_t Duty_State;
} Duty_LPF;
Duty_LPF Duty_LPF_Yaw;
Duty_LPF Duty_LPF_Pitch;

uint8_t Flag_LPF_Yaw,Flag_LPF_Pitch;

uint16_t Duty_Yaw,Duty_Pitch;							//占空比
/*----------驱动定义----------*/
extern volatile uint8_t Flag_4Way;	//横向电机标志位
extern volatile uint8_t Flag_Mode;	//纵向电机标志位

extern volatile uint16_t aimYawAngle;		//串口接受横向角度，0~90
extern volatile uint16_t aimPitchAngle;	//串口接受纵向角度，0~90

uint16_t aimYawDuty,aimPitchDuty;				//翻译角度需求到占空比

uint16_t aimYawAngle_Re;		//横向
uint16_t aimPitchAngle_Re;	//纵向

/*----------横向角度定义----------*/
#define YawAngle_Min 1080
#define YawAngle_Max 6900
#define YawAngle_Bas ((YawAngle_Max - YawAngle_Min)/100)
/*----------纵向角度定义----------*/
#define PitchAngle_Min 2800
#define PitchAngle_Max 4800
#define PitchAngle_Bas ((PitchAngle_Max - PitchAngle_Min)/100)

#define Deviation 50										//死区宽度
/*----------初始化----------*/
void motors_init(void)
{
	Duty_LPF_Yaw.Duty_Alpha = 16;
	Duty_LPF_Pitch.Duty_Alpha = 16;
	
	etmr2_1_interrupt();
	Duty_LPF_Yaw.Duty_State = (Duty_LPF_Yaw.Duty_In << 12);
	//置位到中间
	aimYawAngle_Re = 45;
	aimPitchAngle_Re = 45;
}
/*----------LPF----------*/
void DutyLPF(Duty_LPF *Duty_LPF)
{
	Duty_LPF->Duty_State = Duty_LPF->Duty_State + (Duty_LPF->Duty_In - Duty_LPF->Duty_Out)*Duty_LPF->Duty_Alpha;
	Duty_LPF->Duty_Out = (Duty_LPF->Duty_State >> 12);
}

/*----------横向电机控制----------*/
/*
etmr2_1_interrupt
用来捕捉横向直流电机磁编数据，并做处理
得到Duty_Yaw（占空比放大10000倍）
*/
void etmr2_1_interrupt()
{
  if (eTMR_DRV_GetInputCaptureComplete(CAPTURE_INST_1, CAPTURE_CHAN_1))
  {
    Period_1 = (eTMR2->CH[1].PPCNTV) + (eTMR2->CH[1].NPCNTV);
                    
    High_Duty_1 = eTMR2->CH[1].PPCNTV;
                    
    Duty_LPF_Yaw.Duty_In = ( (High_Duty_1*10000) / Period_1 ) ; //放大一万倍，得到一个四位数  
		
    eTMR_DRV_ClearInputCaptureComplete(CAPTURE_INST_1, CAPTURE_CHAN_1);
  }
}

void Duty_Captrue_Yaw()
{
	etmr2_1_interrupt();
	DutyLPF(&Duty_LPF_Yaw);
	if((Duty_LPF_Yaw.Duty_Out != 0) && (Duty_LPF_Yaw.Duty_Out != 10000))
	{
		Duty_Yaw = Duty_LPF_Yaw.Duty_Out;
		Flag_LPF_Yaw = 0;
	}
	else
	{
		Flag_LPF_Yaw++;
		if(Flag_LPF_Yaw > 5)
		{
			/*报错操作*/
		}
	};
}
/*
app_Yaw_angle(void)
角度判断函数
根据当前角度判断电机转向，并且设置到对应角度
输入：目标角度
*/
void app_Yaw_angle(void)
{
	Duty_Captrue_Yaw();
//	aimYawAngle_Re = aimYawAngle;
	aimYawDuty = ((((aimYawAngle_Re*100) / 90) * YawAngle_Bas) + YawAngle_Min);
	/*
	∵
	1600 ~ 6800 <=> 0 ~ 90
	6800 - 1600 = 5200
	∴
	aimYawDuty = (aimYawAngle / 90)*5200 + 1600
	*/
	if(Duty_Yaw > (aimYawDuty + Deviation))
	{
		Flag_4Way = 1;
	}
	else if(Duty_Yaw < (aimYawDuty - Deviation))
	{
		Flag_4Way = 0;
	}
	else if((Duty_Yaw > (aimYawDuty - Deviation)) && (Duty_Yaw < (aimYawDuty + Deviation)))
	{
		Flag_4Way = 2;
	}
	else
	{
		Flag_4Way = 2;
	}
}
/*----------纵向电机控制----------*/
/*
etmr2_0_interrupt
用来捕捉纵向直流电机磁编数据，并做处理
得到Duty_Pitch（占空比放大10000倍）
*/
void etmr2_0_interrupt()
{
    if (eTMR_DRV_GetInputCaptureComplete(CAPTURE_INST_2, CAPTURE_CHAN_2))
  {
    Period_2 = (eTMR2->CH[0].PPCNTV) + (eTMR2->CH[0].NPCNTV);
                    
    High_Duty_2 = eTMR2->CH[0].PPCNTV;
                    
    Duty_LPF_Pitch.Duty_In = ( (High_Duty_2*10000) / Period_2 ) ; //放大一万倍，得到一个四位数
            
    eTMR_DRV_ClearInputCaptureComplete(CAPTURE_INST_2, CAPTURE_CHAN_2);
  }
}
void Duty_Captrue_Pitch()
{
	etmr2_0_interrupt();
	DutyLPF(&Duty_LPF_Pitch);
	if((Duty_LPF_Pitch.Duty_Out != 0) && (Duty_LPF_Pitch.Duty_Out != 10000))
	{
		Duty_Pitch = Duty_LPF_Pitch.Duty_Out;
		Flag_LPF_Pitch = 0;
	}
	else
	{
		Flag_LPF_Pitch++;
		if(Flag_LPF_Pitch > 5)
		{
			/*报错操作*/
		}
	}
}

/*
Long_angle(int angle)
角度判断函数
根据当前角度判断电机转向，并且设置到对应角度
输入：目标角度
*/
/*
∵
2800 ~ 4800 <> 0 ~ 90
4800 - 2800 = 2000
∴
aimPitchDuty = ((((aimPitchAngle_Test*100) / 90) * 20) + 2800);
*/
void app_Pitch_angle(void)
{
	Duty_Captrue_Pitch();
//	aimPitchAngle_Re = aimYawAngle;
	aimPitchDuty = ((((aimPitchAngle_Re*100) / 90) * PitchAngle_Bas) + PitchAngle_Min);
	if(Duty_Pitch > (aimPitchDuty + Deviation))
	{
		Flag_Mode = 0;
	}
	else if(Duty_Pitch < (aimPitchDuty - Deviation))
	{
		Flag_Mode = 1;
	}
	else if((Duty_Pitch > (aimPitchDuty - Deviation)) && (Duty_Pitch < (aimPitchDuty + Deviation)))
	{
		Flag_Mode = 2;
	}
	else
	{
		Flag_Mode = 2;
	}
}